/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_peripheral.cpp
 * @Description  : 实现了设备操作的客户端类，用于与ROS系统中的动作服务器进行通信。
 *                 代码包括初始化动作客户端、处理反馈、完成回调、启动和结束外设操作等功能。
 *                 主要功能包括：
 *                  1. 初始化动作客户端并连接到动作服务器。
 *                  2. 回调函数处理外设操作的反馈和完成状态。
 *                  3. 启动和结束外设操作，确保操作的顺利进行。
 *                  4. 检查外设是否准备好启动，并处理延迟。
 *                  5. 在操作完成时清理资源。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:34:52
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "operation_peripheral.h" 
#include "action_msgs/srv/cancel_goal.hpp"

rclcpp_action::Client<Peripheral>::SharedPtr OpClient::client_= nullptr;

/**
 * @brief 初始化动作客户端
 *
 * 该函数用于初始化OpClient类中的动作客户端，并尝试连接到外围设备的动作服务器。
 *
 * @param nh ROS节点句柄指针，用于与ROS系统通信。
 */
void OpClient::InitAcitonClient(rclcpp::Node::SharedPtr node)
{
    DEBUG_OUT("#1 Waiting for peripheral action server to start.");
    // create the action client
    client_ = rclcpp_action::create_client<Peripheral>(node, "/do_peripheral");

    DEBUG_OUT("#2 Waiting for peripheral action server to start.");
    // if (!client_->wait_for_action_server(std::chrono::seconds(120))) {
    //     DEBUG_ERROR_OUT("Do_peripheral action server not available after waiting");
    //     return;
    // }

    DEBUG_WARN_OUT("Peripheral Action server started");
}

////////////////////////////OperationPeripheral ///////////////////////////////////////////

/**
 * @brief 回调函数，用于处理外设的反馈
 *
 * 该函数作为回调函数，用于处理外设操作完成后的反馈。
 *
 * @param feedback 外设操作的反馈数据指针
 */
void OperationPeripheral::FeedbackCb(GoalHandlePeripheral::SharedPtr,
    const std::shared_ptr<const Peripheral::Feedback> feedback)
{
    int cur_state = feedback->state;
    float cur_percent = feedback->percent_complete;
    (void)(cur_percent);

    if (cur_percent >= 0.95f || cur_percent < 0.1f) {
        DEBUG_OUT("cur_state:" << cur_state << ",cur_percent:" << cur_percent);
    }
}

/**
 * @brief 操作外设完成回调函数
 *
 * 当外设操作完成时，调用此回调函数进行处理。
 *
 * @param state 操作状态
 * @param result 操作结果
 */
void OperationPeripheral::DoneCb(const GoalHandlePeripheral::WrappedResult & result)
{
    std::lock_guard<std::mutex> locker(mutex_);
    auto& result_msg = result.result;

    DEBUG_WARN_OUT("Peripheral action" << result_msg->result_action.actiondescription << 
        " Success :" << result_msg->success << ",message " << result_msg->message);

    int peripheral_result = result_msg->success;

    // 0:normal success  1:failed 2:canceled  3:other
    switch (peripheral_result) {
        case 0:
            finished_ = true;
            DEBUG_WARN_OUT("action " << result_msg->result_action.actiondescription << " finsh ok");
            break;

        case 1:
            stage_ = OpInit;
            DEBUG_ERROR_OUT("action " << result_msg->result_action.actiondescription << " do failed: ready restart");
            break;

        case 2:
            stage_ = OpInit;
            DEBUG_ERROR_OUT("action " << result_msg->result_action.actiondescription << " canceled: ready restart");
            break;

        case 3:
            stage_ = OpInit;
            DEBUG_ERROR_OUT("action " << result_msg->result_action.actiondescription << " not succeed: ready restart");
            break;

        default:
            stage_ = OpInit;
            DEBUG_ERROR_OUT("action " << result_msg->result_action.actiondescription << "unknown: ready restart");
            break;
    }
}

/**
 * @brief 当目标刚刚变为活动状态时调用的回调函数
 *
 * 当目标刚刚变为活动状态时，会调用此函数，并输出一条警告信息。
 */
void OperationPeripheral::ActiveCb(GoalHandlePeripheral::SharedPtr goal_handle)
{
    if (!goal_handle) {
        DEBUG_ERROR_OUT("Goal was rejected by server");
    } else {
        DEBUG_OUT("Goal accepted by server, waiting for result");
    }
}

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "OperationPeripheral".
// #1  blockingtype:
//  Enum {NOTE, SOFT, HARD}:
//  "NONE"- allows driving and other actions;
//  "SOFT"- allows other actions, but not driving;
//  "HARD"- is the only allowed action at that time.
/**
 * @brief 构造函数
 *
 * 该构造函数用于初始化 OperationPeripheral 类的实例，并通过参数 action 接收一个 agv_msgs::msg::Action 类型的消息。
 *
 * @param action agv_msgs::msg::Action 类型的消息，包含操作指令。
 */
OperationPeripheral::OperationPeripheral(const agv_msgs::msg::Action& action)
    : Operation(action)
{
}

/**
 * @brief 析构函数，用于清理资源
 *
 * 当OperationPeripheral对象被销毁时，如果OpClient存在，则调用其stopTrackingGoal方法停止跟踪目标，并输出调试信息。
 */
OperationPeripheral::~OperationPeripheral()
{
}

//
//   The routine to be called in each working cycle.
//
// void OperationPeripheral::CycleRoutine()
// {
// }
/**
 * @brief 初始化操作外设对象
 *
 * 初始化操作外设对象，并检查是否与服务器连接成功。
 *
 * @return 如果初始化成功返回true，否则返回false。
 */
bool OperationPeripheral::Init()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    delay_count_ = 0;
    if (!OpClient::client_) {
        DEBUG_ERROR_OUT("action client is null");
        return false;
    }

    if (!OpClient::client_->action_server_is_ready()) {
        DEBUG_ERROR_OUT("action client isServerConnected is failed");
        return false;
    }
    DEBUG_OUT("Op:Init is ok; ready to clear(end) last action");
    EndAction(); // add at 2023-2-13

    return true;
}

/**
 * @brief 检查外设是否准备好启动
 *
 * 此函数检查外设是否准备好启动。
 * 将调用TryCycleMarkCheck函数检查周期标记的有效性。
 * 如果周期标记无效，将输出错误信息并返回false。
 * 如果周期标记有效，将计算延迟时间并检查延迟计数是否达到最大值。
 * 如果达到最大值，则输出延迟时间和延迟计数并返回true；否则返回false。
 *
 * @return 如果外设准备好启动，则返回true；否则返回false。
 */
bool OperationPeripheral::ReadyStart()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif

    if (!TryCycleMarkCheck()) {
        DEBUG_ERROR_OUT("OperationPeripheral: qrId is not Valid");
        return false;
    }

    const int delay_ms = 800, cycle = 50;
    const int max_delay_count = delay_ms / cycle; // 800ms /50ms = 16;
    if (delay_count_++ >= max_delay_count) {
        DEBUG_OUT("before start delay ms:" << delay_ms << ",delay_count_:" << delay_count_);
        return true;
    }

    return false;
}

/**
 * @brief 开始执行动作
 *
 * 该函数用于启动外设操作。首先会检查是否已经连接服务器，如果未连接，则直接返回失败。
 * 如果已连接，则创建一个动作目标，并发送该目标到服务器。
 *
 * @return 如果成功启动动作，则返回true；否则返回false。
 */
bool OperationPeripheral::StartAction()
{
    std::lock_guard<std::mutex> locker(mutex_);

    finished_ = false;
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    if (!OpClient::client_ || !OpClient::client_->action_server_is_ready()) {
        DEBUG_ERROR_OUT("StartAction failed");
        return false;
    }

    try {
        auto goal_msg = Peripheral::Goal();
        goal_msg.goal_action = action_;        // 直接发送调度格式的action
        auto send_goal_options = rclcpp_action::Client<Peripheral>::SendGoalOptions();
        send_goal_options.goal_response_callback =
            std::bind(&OperationPeripheral::ActiveCb, this, std::placeholders::_1);
        send_goal_options.feedback_callback =
            std::bind(&OperationPeripheral::FeedbackCb, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback =
            std::bind(&OperationPeripheral::DoneCb, this, std::placeholders::_1);

        auto goal_handle_future = OpClient::client_->async_send_goal(goal_msg, send_goal_options);
    
        DEBUG_OUT("Operation start ok");
    } catch (const std::exception &e) {
        DEBUG_ERROR_OUT("action:" << e.what());
    } catch (...) {
        DEBUG_ERROR_OUT("start action failed");
    }

    return true;
}

/**
 * @brief 外设执行操作
 *
 * @return 返回是否已完成操作的标志。
 */
bool OperationPeripheral::Work()
{
    std::lock_guard<std::mutex> locker(mutex_);

#ifdef AGV_DEBUG_NAV
    return m_uTotalCount++ < 100; // 5s-500
#endif

    return finished_;
}

/**
 * @brief 结束外设操作
 *
 * 此函数用于结束当前的外设操作。
 * 如果 OpClient::client_ 存在，则检查其状态是否为 actionlib::SimpleClientGoalState::SUCCEEDED。
 * 如果是，则输出调试信息并取消所有目标，停止跟踪目标（注意：停止跟踪目标很重要，如果不停止，
 * 可能会导致在回调中持续进入，从而导致崩溃）。
 *
 * @return 返回 true，表示操作结束。
 */
bool OperationPeripheral::EndAction()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    //.if (OpClient::client_ && OpClient::client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    if (OpClient::client_) {
        auto future = OpClient::client_->async_cancel_all_goals();
        DEBUG_OUT("#2 stopTrackingGoal");
    }
    return true;
}
